import json
import re
import scipy.io
import numpy as np

import matplotlib.colors as mcolors

def read_mat_file(file_path):
    try:
        return scipy.io.loadmat(file_path)
    except Exception as e:
        print(f"Error reading {file_path}: {e}")
        return None
    
def remap_histogram(image):
    flattened = image.flatten()
    unique_values = np.unique(flattened)
    mapping = {old_val: new_val for new_val, old_val in enumerate(unique_values)}
    remapped_flattened = np.array([mapping[val] for val in flattened])
    remapped_image = remapped_flattened.reshape(image.shape)
    normalized_image = ((remapped_image - remapped_image.min()) / \
                        (remapped_image.max() - remapped_image.min()) * 255).astype(np.uint8)
    return normalized_image

def load_global_label(label_file):
    with open(label_file, 'r', encoding='utf-8') as f:
        label_data = json.load(f)

    polygons = []
    for shape in label_data.get("shapes", []):
        if shape.get("shape_type") == "polygon":
            polygons.append(shape.get("points", []))
    return polygons

def natural_key(text):
    return [int(c) if c.isdigit() else c for c in re.split(r'(\d+)', text)]

def latlonh_to_ecef(lat, lon, h):
    a = 6378137.0          # 赤道半径
    e2 = 6.69437999014e-3  # 偏心率平方

    lat_rad = np.radians(lat)
    lon_rad = np.radians(lon)

    sin_lat = np.sin(lat_rad)
    cos_lat = np.cos(lat_rad)
    sin_lon = np.sin(lon_rad)
    cos_lon = np.cos(lon_rad)

    N = a / np.sqrt(1 - e2 * sin_lat**2)

    X = (N + h) * cos_lat * cos_lon
    Y = (N + h) * cos_lat * sin_lon
    Z = (N*(1 - e2) + h) * sin_lat

    return X, Y, Z

def ecef_to_enu(X, Y, Z, lat0, lon0, h0):
    X0, Y0, Z0 = latlonh_to_ecef(lat0, lon0, h0)

    lat0_rad = np.radians(lat0)
    lon0_rad = np.radians(lon0)

    sin_lat0 = np.sin(lat0_rad)
    cos_lat0 = np.cos(lat0_rad)
    sin_lon0 = np.sin(lon0_rad)
    cos_lon0 = np.cos(lon0_rad)

    R = np.array([
        [-sin_lon0,          cos_lon0,         0        ],
        [-cos_lon0*sin_lat0, -sin_lon0*sin_lat0, cos_lat0],
        [cos_lon0*cos_lat0,  sin_lon0*cos_lat0,  sin_lat0]
    ])

    dX = X - X0
    dY = Y - Y0
    dZ = Z - Z0

    enu = R.dot(np.vstack((dX, dY, dZ)))
    return enu[0, :].squeeze(), enu[1, :].squeeze(), enu[2, :].squeeze()

def rotation_matrix_from_euler(roll, pitch, heading):
    r = np.radians(roll)
    p = np.radians(pitch)
    h = np.radians(-heading)

    R_x = np.array([
        [1,       0,        0      ],
        [0, np.cos(r), -np.sin(r)],
        [0, np.sin(r),  np.cos(r)]
    ])

    R_y = np.array([
        [ np.cos(p), 0, np.sin(p)],
        [0,          1,         0],
        [-np.sin(p), 0, np.cos(p)]
    ])

    R_z = np.array([
        [ np.cos(h), -np.sin(h), 0],
        [ np.sin(h),  np.cos(h), 0],
        [          0,          0, 1]
    ])

    R = R_z.dot(R_y).dot(R_x)
    return R

def local_to_global_coords(X, Y, Z, lat, lon, h, pitch, roll, heading, lat0, lon0, h0):
    plat_X, plat_Y, plat_Z = latlonh_to_ecef(lat, lon, h)
    plat_e, plat_n, plat_u = ecef_to_enu(plat_X, plat_Y, plat_Z, lat0, lon0, h0)

    local_points = np.vstack((X, Y, Z))

    R = rotation_matrix_from_euler(roll, pitch, heading)

    global_points = R.dot(local_points) + np.array([[plat_e], [plat_n], [plat_u]])

    X_global = global_points[0, :]
    Y_global = global_points[1, :]
    Z_global = global_points[2, :]

    return X_global.squeeze(), Y_global.squeeze(), Z_global.squeeze()

def global_to_local_coords(X, Y, Z, lat, lon, h, pitch, roll, heading, lat0, lon0, h0):
    plat_X, plat_Y, plat_Z = latlonh_to_ecef(lat, lon, h)
    e, n, u = ecef_to_enu(plat_X, plat_Y, plat_Z, lat0, lon0, h0)

    global_xyz = np.vstack((X, Y, Z))

    global_xyz_centered = global_xyz - np.array([e, n, u]).reshape(3, 1)

    R = rotation_matrix_from_euler(roll, pitch, heading)

    local_xyz = R.T.dot(global_xyz_centered)

    X = local_xyz[0, :]
    Y = local_xyz[1, :]
    Z = local_xyz[2, :]

    return X.squeeze(), Y.squeeze(), Z.squeeze()

def convert_pixels_to_global(polygons, meta):
    x_min, y_min = meta["left_bottom"]
    x_max, y_max = meta["right_top"]
    resolution = meta["resolution"]

    trans_polygons = []
    global_coordinates = []
    for polygon in polygons:
        global_coordinates = []
        for point in polygon:
            pixel_x, pixel_y = point
            global_x = x_min + pixel_x / resolution
            global_y = y_max - pixel_y / resolution
            global_coordinates.append([global_x, global_y])

        trans_polygons.append(global_coordinates.copy())
    return trans_polygons

def filter_data(data, x_range, y_range, filter_noise = False):
    X = data['x']
    Y = data['y']

    mask = (X >= x_range[0]) & (X <= x_range[1]) & (Y >= y_range[0]) & (Y <= y_range[1])
    
    data = {key: np.array(value)[mask] for key, value in data.items()}

    if filter_noise:
        azi = data['aziLinearlityMap']
        ele = data['eleLinearlityMap']
        err = data['errorMap']

        mask = (azi > 0.9) & (ele > 0.9) & (err < 0.2 * np.sqrt(data['x']**2 + data['y']**2))
        data = {key: value[mask] for key, value in data.items()}

    return data

def filter_z(data):
    Z = data['z']

    mask = (Z != 0) & (Z < 3) & (Z > -1)
    
    data = {key: np.array(value)[mask] for key, value in data.items()}

    return data

def compute_rgb(Z, s, DN, use_s = False):
    Z_trimmed = Z[Z <= np.percentile(Z, 95)]
    if Z_trimmed.size == 0:
        Z_trimmed = Z
    Z_normalized = (Z - Z_trimmed.min()) / (Z_trimmed.max() - Z_trimmed.min() + 1e-8)
    hue = 2/3 * (1 - Z_normalized)
    hue = np.clip(hue, 0, 2/3)

    value = 1 - (DN - DN.min()) / (DN.max() - DN.min())
    saturation = s / s

    if use_s:
        value =  (s - s.min()) / (s.max() - s.min())
        saturation = 1 - (DN - DN.min()) / (DN.max() - DN.min())

    hsv = np.stack((hue, saturation, value), axis=1)
    rgb = mcolors.hsv_to_rgb(hsv)
    return rgb